#include "crossloc.h"



Crossloc::Crossloc()
{
  RobPos.ReSize(3,1);
  RobPPos.ReSize(3,3);
  RobPhyChar.ReSize(4,1);
  RobPPhyChar.ReSize(4,4);
  PhyCharMinDistTol = 1;  //Min Dist Tolerance for Physical Characteristics
}


float Crossloc::BhaDist(NEWMAT::Matrix M1,NEWMAT::Matrix P1,NEWMAT::Matrix M2,NEWMAT::Matrix P2)
{
  float dist;
  NEWMAT::Matrix DistM;
  DistM = DistM.t();
  NEWMAT::Matrix P;
  P=(1/2)*(P1+P2);
  float detP1 = P1.Determinant();
  float detP2 = P2.Determinant();
  float detP = P.Determinant();
  DistM = (1/8)*(M1-M2).t()*P.i()*(M1-M2);
  dist = DistM.AsScalar()+(1/2)*log(detP/sqrt(detP1+detP2));
  return dist;


}

void Crossloc::RobPosCallback()
{


}

void Crossloc::Position_EKF()
{
  //Update Robot Position based on inputs from sensors (internal an external) and multiloc


}
void Crossloc::ObjectList_Callback(const MULTILOC::DetectedObjectsMsg::ConstPtr& msg)
{

  int obj_num = msg->List.size();
  float PhyCharDist[obj_num];
  float PosDist[obj_num];
  std::vector<MULTILOC::DynamicObjectMsg>::iterator it;
  std::vector<MULTILOC::DynamicObjectMsg> L = msg->List;
  int i =0;
  for(it=L.begin();it<L.end();it++)
  {
    NEWMAT::Matrix ObjPhyChar(4,1);
    NEWMAT::Matrix ObjPPhyChar(4,4);
    NEWMAT::Matrix ObjPos(3,1);
    NEWMAT::Matrix ObjPPos(3,3);
    //Variables and covariance from physical characteristics
    ObjPhyChar(1,1) = it->size;
    ObjPhyChar(2,1) = it->RColor;
    ObjPhyChar(3,1) = it->GColor;
    ObjPhyChar(4,1) = it->BColor;

    ObjPPhyChar.Row(1) << it->Pcovar_size[3]<<it->Pcovar_size[4]<<it->Pcovar_size[5]<<it->Pcovar_size[6];
    ObjPPhyChar.Row(2) << it->Pcovar_R[3]<<it->Pcovar_R[4]<<it->Pcovar_R[5]<<it->Pcovar_R[6];
    ObjPPhyChar.Row(3) << it->Pcovar_G[3]<<it->Pcovar_G[4]<<it->Pcovar_G[5]<<it->Pcovar_G[6];
    ObjPPhyChar.Row(4) << it->Pcovar_B[3]<<it->Pcovar_B[4]<<it->Pcovar_B[5]<<it->Pcovar_B[6];

    //Variables and covariance from position
    ObjPos(1,1) = it->Pos_x;
    ObjPos(2,1) = it->Pos_y;
    ObjPos(3,1) = it->Pos_z;
    ObjPPos.Row(1) << it->Pcovar_x[0] << it->Pcovar_x[1] << it->Pcovar_x[2];
    ObjPPos.Row(2) << it->Pcovar_y[0] << it->Pcovar_y[1] << it->Pcovar_y[2];
    ObjPPos.Row(3) << it->Pcovar_z[0] << it->Pcovar_z[1] << it->Pcovar_z[2];

    PhyCharDist[i] = BhaDist(RobPhyChar,RobPPhyChar,ObjPhyChar,ObjPPhyChar);
    PosDist[i] = BhaDist(RobPos,RobPPos,ObjPos,ObjPPos);
    i++;
  }
//Check if the closest pair is feasible
  float PosMinDist = PosDist[0];
  int i_PosMinDist = 0;
  for(int i=1;i < obj_num;i++)
  {
    if(PosMinDist > PosDist[i])
    {
      PosMinDist = PosDist[i];
      i_PosMinDist = i;
    }
  }
  //Check the physical characteristics distance.
  float PhyCharMinDist = PhyCharDist[i];
  if (PhyCharMinDist < PhyCharMinDistTol)
  {
// TODO Que pasa si la pareja es seleccionada.

  }

}
